
#include "processcloud.hpp"

void Process_Cloud::add_input_point(pcl::PointXYZ p )
{
  input_cloud.push_back(p);
}
void Process_Cloud::clear_input_point(void)
{
  input_cloud.clear();
}
pcl::PointCloud <pcl::PointXYZ>::Ptr Process_Cloud::getinput_cloudptr(void)
{
 return input_cloud.makeShared();
}
pcl::PointCloud <pcl::PointXYZ>::Ptr Process_Cloud::getoutput_cloudptr(void)
{
 return output_cloud.makeShared();
}
void Process_Cloud::vgrid(double grid)
{
 pcl::VoxelGrid<pcl::PointXYZ> sor;
 
  sor.setInputCloud (input_cloud.makeShared());
  sor.setLeafSize (grid,grid,grid);
    pcl::PointCloud<pcl::PointXYZ>::Ptr downsampled(new pcl::PointCloud<pcl::PointXYZ>);
   // sor.filter(*downsampled);
  sor.filter (output_cloud);
  //std::cout<<"inputcloud= "<<input_cloud.points.size()<<" outfcloud= "<<output_cloud.points.size()<<std::endl;

}
//点云平移和旋转
void Process_Cloud::convert_cloud(float tick)
{
   //double theta=M_PI*tick;
   double theta=0;
   Eigen::Affine3f transform = Eigen::Affine3f::Identity();
   // Define a translation of 2.5 meters on the x axis.
   transform.translation() << 0.0,tick*5, 0.0;
    // The same rotation matrix as before; theta radians around Z axis
   transform.rotate (Eigen::AngleAxisf (theta, Eigen::Vector3f::UnitZ()));
   pcl::transformPointCloud (input_cloud, output_cloud, transform);
}
void Process_Cloud::swap(void)
{
  pcl::PointCloud <pcl::PointXYZ> temp_cloud;
  std::size_t size;
  size=input_cloud.points.size();
  temp_cloud.clear();
  for(int i=0;i<size;++i)
    temp_cloud.points.push_back(input_cloud.points[i]);
  input_cloud.clear();
  size= output_cloud.points.size(); 
  for(int i=0;i<size;++i)
    input_cloud.points.push_back(output_cloud.points[i]);
  output_cloud.clear();
  size= temp_cloud.points.size();
  for(int i=0;i<size;++i)
    output_cloud.points.push_back(temp_cloud.points[i]);  
}
void Process_Cloud::my_harris_3d(void)
{
  vgrid(0.1);
  swap();
  	//-----------------------------Harris关键点提取----------------------------------
	//注意此处PCL的point类型设置为<pcl::PointXYZI>,即除了x、y、z坐标还必须包含强度信息
	//因为Harris的评估值保存在输出点云的(I)分量中，Harris输出点云中的(I)非传统点云中的
	//强度信息，因此后续在保存和可视化输出点云的时候需要通过点的索引来重新获取。
	//-------------------------------------------------------------------------------
	pcl::HarrisKeypoint3D <pcl::PointXYZ, pcl::PointXYZI> harris;
	pcl::PointCloud<pcl::PointXYZI>::Ptr Harris_keypoints(new pcl::PointCloud<pcl::PointXYZI>);
  //const pcl::PointCloud<pcl::PointXYZI>::Ptr cloudRef = static_cast<const PointCloudInConstPtr>(&input_cloud);
  harris.setInputCloud(input_cloud.makeShared());     // 提供指向输入数据集的指针
	harris.setMethod(harris.LOWE);   // 设置要计算响应的方法（可以不设置）
	harris.setRadius(0.1);          // 设置法线估计和非极大值抑制的半径。
	harris.setRadiusSearch(0.1);    // 设置用于关键点检测的最近邻居的球半径
	harris.setNonMaxSupression(true);// 是否应该应用非最大值抑制
	harris.setThreshold(0.01);     // 设置角点检测阈值，只有当非极大值抑制设置为true时才有效
	harris.setRefine(true);          // 检测到的关键点是否需要细化，设置为true时，关键点为点云中的点
	harris.setNumberOfThreads(6);    // 初始化调度程序并设置要使用的线程数
	harris.compute(*Harris_keypoints);
  
  int size;
  //-----------------------获取关键点的索引-------------------------
	pcl::PointIndicesConstPtr keypoints2_indices = harris.getKeypointsIndices();

  size = Harris_keypoints->points.size();
  pcl::copyPointCloud(input_cloud,*keypoints2_indices,output_cloud);
  std::cout<<"harris size="<<size<<std::endl;
}





